%Code to randomise placement of some object and then to detect the
%placement of it through MLE
%Authors: Nathan Rich Chris Chester
clear
clc
cla
clf reset

% Pre-defining Arrays
R1 = zeros(4000,1);
R2 = zeros(4000,1);
S = zeros(4000,1);
Time = zeros(4000,1);
Out1 = zeros(4000,1);
Out2 = zeros(4000,1);
xguess = zeros(10,1);
yguess = zeros(10,1);

a1 = 0; %Position of Antenna relative to x axis, 0 on y axis
a2 = 1e-6; %antennas 1mm apart

freqwave = 8e9;    %8GHz Cosine wave used 
velowave = 3e5; %speed of light km/s
lengthwave = velowave/freqwave; 

%Both antenna first transmit a cosine then wait to receive it back from
%object.
srate = 80; %80 samples per wave

%With ideal LPF 
d  = fdesign.lowpass('Fp,Fst,Ap,Ast',0.001,0.01,1e-5,1e-4,'linear');
Hd = design(d, 'equiripple');

%Creating Signal to be sent
for I = 1:4000
        S(I) = cos(I*2*pi/srate); %Sent Signal
        Time(I) = I/(freqwave*srate)*1e9;  %Scales to ns
end
    
for J = 1:1000
    
    %Obtaining the recieved signal at antennas 1 and 2 
    [R1, R2, delay1, delay2] = radar_signal(S,a1,a2,freqwave,velowave,srate); 
    
    
    %Using cos(a)cos(a+b) = 1/2 cos(b) + 1/2 cos(2a), to find cos(b)  
    Out1 = filter(Hd.Numerator,1,S.*R1);
    Out2 = filter(Hd.Numerator,1,S.*R2);

    %Averaging the DC of the filter (MLE estimate)
    Out1ave = mean(Out1(1000:4000));   %First 1000 terms ignored to allow
    Out2ave = mean(Out2(1000:4000));   %for the signal to converge

    %Filter has a DC gain of 0.9067 and 1/2 scaling
    phase1 = acos (Out1ave*2);
    phase2 = acos (Out2ave*2);

    %Checks to see if acos has chosen the right angle out of the two possible
    %choices 
    if (R1(srate*1/4) < 0) %sees if the wave is -ve at the pi/2 point
        phase1 = 2* pi - phase1; % If the wave is -ve then the phase delay > pi
    end

    if (R2(srate*1/4) < 0)
        phase2 = 2* pi - phase2;
    end

    %Finds distance
    phasedif = (phase1-phase2);

    length_from_a1 = delay1*velowave/2;
    length_from_a2 = delay2*velowave/2;
    distdif = phasedif/(2*pi) * lengthwave /2;
   
    theta = acos(distdif/(a2-a1));

    xguess(J) = abs(length_from_a1*cos(theta));
    yguess(J) = abs(length_from_a1*sin(theta));
end

x_new = mean(abs(xguess))
y_new = mean(abs(yguess))

[talnums,talvalues] = tally(xguess);

figure(1)
plot(Time(1:200),R1(1:200), 'b')
hold on
plot(Time(1:200),R2(1:200), 'r')
hold on
plot(Time(1:200) ,S(1:200),'g')

title('Recieved signals vs original')
xlabel('Time (ns)')
ylabel('Amplitude')
legend('Received Signal 1', 'Received Signal 2', 'Sent Signal')


figure(2)
plot(Time,Out1)
title('Filtered Output')
xlabel('Time (ns)')
ylabel('Amplitude')

% Used to plot the probability distribution of the results
figure(3)
bar(talvalues,talnums)
